% main.m

function main()
    
    clear all;
    load("ConstantDefinition.mat", "FrameRate");
    load("ThetaData.mat", "Angel_theta24");

    t=timer('Name','MotionTimer',...
        'Period',1/FrameRate,...
        'StartDelay',1,...
        'ExecutionMode','fixedRate',...
        'BusyMode','queue');

    t.TimerFcn = @MotionCallBack;
    t.UserData = 1;

    try
        input("Press Enter to start position!")
        StartMotion(Angel_theta24(:,1));
        input("Press Enter to start moving!");
        start(t);
        % pause(10);
        input("Press Enter to stop moving!")
        stop(t);
    catch ME
        stop(t)
        EndMotion();
        disp("Error in running timer")
        rethrow(ME)
    end

    EndMotion();
end

function StartMotion(start_theta)

    lib_name = '';
    
    if strcmp(computer, 'PCWIN')
      lib_name = 'dxl_x86_c';
    elseif strcmp(computer, 'PCWIN64')
      lib_name = 'dxl_x64_c';
    elseif strcmp(computer, 'GLNX86')
      lib_name = 'libdxl_x86_c';
    elseif strcmp(computer, 'GLNXA64')
      lib_name = 'libdxl_x64_c';
    elseif strcmp(computer, 'MACI64')
      lib_name = 'libdxl_mac_c';
    end
    
    % Load Libraries
    if ~libisloaded(lib_name)
        [notfound, warnings] = loadlibrary(lib_name, 'dynamixel_sdk.h', 'addheader', 'port_handler.h', 'addheader', 'packet_handler.h');
    end
    
    % Control table address
    ADDR_PRO_TORQUE_ENABLE       = 64;         % Control table address is different in Dynamixel model
    ADDR_PRO_GOAL_POSITION       = 116;
    ADDR_PRO_PRESENT_POSITION    = 132;
    
    % Protocol version
    PROTOCOL_VERSION            = 2.0;          % See which protocol version is used in the Dynamixel
    
    load("ConstantDefinition.mat", "DXL_ID", "BAUDRATE", "DEVICENAME", "ESC_CHARACTER")
    
    TORQUE_ENABLE               = 1;            % Value for enabling the torque
    TORQUE_DISABLE              = 0;            % Value for disabling the torque
    
    COMM_SUCCESS                = 0;            % Communication Success result value
    COMM_TX_FAIL                = -1001;        % Communication Tx Failed
    
    % Initialize PortHandler Structs
    % Set the port path
    % Get methods and members of PortHandlerLinux or PortHandlerWindows
    port_num = portHandler(DEVICENAME);

    save("ConstantDefinition.mat", "port_num", "-append");
    
    % Initialize PacketHandler Structs
    packetHandler();
    
    dxl_comm_result = COMM_TX_FAIL;           % Communication result    
    dxl_error = 0;                              % Dynamixel error
    
    % Open port
    if (openPort(port_num))
        fprintf('Succeeded to open the port!\n');
    else
        unloadlibrary(lib_name);
        fprintf('Failed to open the port!\n');
        input('Press any key to terminate...\n');
        return;
    end
    
    
    % Set port baudrate
    if (setBaudRate(port_num, BAUDRATE))
        fprintf('Succeeded to change the baudrate!\n');
    else
        unloadlibrary(lib_name);
        fprintf('Failed to change the baudrate!\n');
        input('Press any key to terminate...\n');
        return;
    end

    % Enable Dynamixel Torque
    % dxl1
    write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID(1), ADDR_PRO_TORQUE_ENABLE, TORQUE_ENABLE);
    dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION);
    dxl_error = getLastRxPacketError(port_num, PROTOCOL_VERSION);
    if dxl_comm_result ~= COMM_SUCCESS
        fprintf('%s\n', getTxRxResult(PROTOCOL_VERSION, dxl_comm_result));
    elseif dxl_error ~= 0
        fprintf('%s\n', getRxPacketError(PROTOCOL_VERSION, dxl_error));
    else
        fprintf('Dynamixel1 has been successfully connected \n');
    end
    % dxl2
    write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID(2), ADDR_PRO_TORQUE_ENABLE, TORQUE_ENABLE);
    dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION);
    dxl_error = getLastRxPacketError(port_num, PROTOCOL_VERSION);
    if dxl_comm_result ~= COMM_SUCCESS
        fprintf('%s\n', getTxRxResult(PROTOCOL_VERSION, dxl_comm_result));
    elseif dxl_error ~= 0
        fprintf('%s\n', getRxPacketError(PROTOCOL_VERSION, dxl_error));
    else
        fprintf('Dynamixel2 has been successfully connected \n');
    end

    % Write start position
    write4ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID(1), ADDR_PRO_GOAL_POSITION, typecast(int32(start_theta(1)), 'uint32'));
    dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION);
    dxl_error = getLastRxPacketError(port_num, PROTOCOL_VERSION);
    if dxl_comm_result ~= COMM_SUCCESS
        fprintf('%s\n', getTxRxResult(PROTOCOL_VERSION, dxl_comm_result));
    elseif dxl_error ~= 0
        fprintf('%s\n', getRxPacketError(PROTOCOL_VERSION, dxl_error));
    end
    write4ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID(2), ADDR_PRO_GOAL_POSITION, typecast(int32(h(2)), 'uint32'));
    dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION);
    dxl_error = getLastRxPacketError(port_num, PROTOCOL_VERSION);
    if dxl_comm_result ~= COMM_SUCCESS
        fprintf('%s\n', getTxRxResult(PROTOCOL_VERSION, dxl_comm_result));
    elseif dxl_error ~= 0
        fprintf('%s\n', getRxPacketError(PROTOCOL_VERSION, dxl_error));
    end
    % Wait until motion get to target position
    while 1
        % Read present position
        dxl1_present_position = read4ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID(1), ADDR_PRO_PRESENT_POSITION);
        dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION);
        dxl_error = getLastRxPacketError(port_num, PROTOCOL_VERSION);
        if dxl_comm_result ~= COMM_SUCCESS
            fprintf('%s\n', getTxRxResult(PROTOCOL_VERSION, dxl_comm_result));
        elseif dxl_error ~= 0
            fprintf('%s\n', getRxPacketError(PROTOCOL_VERSION, dxl_error));
        end
        dxl2_present_position = read4ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID(2), ADDR_PRO_PRESENT_POSITION);
        dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION);
        dxl_error = getLastRxPacketError(port_num, PROTOCOL_VERSION);
        if dxl_comm_result ~= COMM_SUCCESS
            fprintf('%s\n', getTxRxResult(PROTOCOL_VERSION, dxl_comm_result));
        elseif dxl_error ~= 0
            fprintf('%s\n', getRxPacketError(PROTOCOL_VERSION, dxl_error));
        end

        fprintf('[ID:%03d] GoalPos:%03d  PresPos:%03d\n', DXL_ID(1), start_theta(1), typecast(uint32(dxl1_present_position), 'int32'));
        fprintf('[ID:%03d] GoalPos:%03d  PresPos:%03d\n', DXL_ID(2), start_theta(2), typecast(uint32(dxl2_present_position), 'int32'));

        if abs(start_theta(1) - typecast(uint32(dxl1_present_position), 'int32')) <= 20 && ...
             abs(start_theta(2) - typecast(uint32(dxl2_present_position), 'int32')) <= 20
            break;
        end
    end
end

function EndMotion()

    lib_name = '';
    
    if strcmp(computer, 'PCWIN')
      lib_name = 'dxl_x86_c';
    elseif strcmp(computer, 'PCWIN64')
      lib_name = 'dxl_x64_c';
    elseif strcmp(computer, 'GLNX86')
      lib_name = 'libdxl_x86_c';
    elseif strcmp(computer, 'GLNXA64')
      lib_name = 'libdxl_x64_c';
    elseif strcmp(computer, 'MACI64')
      lib_name = 'libdxl_mac_c';
    end

    % Control table address
    ADDR_PRO_TORQUE_ENABLE       = 64;         % Control table address is different in Dynamixel model
    
    % Protocol version
    PROTOCOL_VERSION            = 2.0;          % See which protocol version is used in the Dynamixel
    
    load("ConstantDefinition.mat", "DXL_ID", "port_num");
    
    TORQUE_ENABLE               = 1;            % Value for enabling the torque
    TORQUE_DISABLE              = 0;            % Value for disabling the torque
    
    COMM_SUCCESS                = 0;            % Communication Success result value
    COMM_TX_FAIL                = -1001;        % Communication Tx Failed
    
    dxl_comm_result = COMM_TX_FAIL;           % Communication result    
    dxl_error = 0;                              % Dynamixel error

    % Disable Dynamixel Torque
    % dxl1
    write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID(1), ADDR_PRO_TORQUE_ENABLE, TORQUE_DISABLE);
    dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION);
    dxl_error = getLastRxPacketError(port_num, PROTOCOL_VERSION);
    if dxl_comm_result ~= COMM_SUCCESS
        fprintf('%s\n', getTxRxResult(PROTOCOL_VERSION, dxl_comm_result));
    elseif dxl_error ~= 0
        fprintf('%s\n', getRxPacketError(PROTOCOL_VERSION, dxl_error));
    end
    % dxl2
    write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID(2), ADDR_PRO_TORQUE_ENABLE, TORQUE_DISABLE);
    dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION);
    dxl_error = getLastRxPacketError(port_num, PROTOCOL_VERSION);
    if dxl_comm_result ~= COMM_SUCCESS
        fprintf('%s\n', getTxRxResult(PROTOCOL_VERSION, dxl_comm_result));
    elseif dxl_error ~= 0
        fprintf('%s\n', getRxPacketError(PROTOCOL_VERSION, dxl_error));
    end
    
    % Close port
    closePort(port_num);
    
    % Unload Library
    unloadlibrary(lib_name);
end
